#include "Matrix.h"

Row::Row(const fp _m1, const fp _m2, const fp _m3, const fp _m4) { Set(_m1,_m2,_m3,_m4); };
Row::Row(const Row& r) { Set(r.m1,r.m2,r.m3,r.m4); };

void Row::Set(const fp _m1, const fp _m2, const fp _m3, const fp _m4)
{
  m1 = _m1;
  m2 = _m2;
  m3 = _m3;
  m4 = _m4;
};

void Row::Mul(const fp k) { for (ui32 x = 0; x < 4; x++) m[x] *= k; };
void Row::Div(const fp k) { if (k != 0.0f) Mul(1.0f / k); };

Matrix::Matrix()
{
  memset(z, NULL, sizeof(z));
  for (ui32 i = 0; i < 3; i++) z[i] = new Row();
}
Matrix::~Matrix() { for (ui32 i = 0; i < 3; i++) delete z[i]; }

bool Matrix::Check(const ui32 i, const ui32 j) const
{
  return (i != j &&
          i < 3 &&
          j < 3 &&
          z[i] != NULL &&
          z[j] != NULL);
};

// exchange j->i,i->j
void Matrix::Swap(const ui32 i, const ui32 j)
{
	if (Check(i,j) == false) return;
	
	Row *old = z[j];
	z[j] = z[i];
	z[i] = old;
};

// copy jk->i
void Matrix::Add(const ui32 i, const ui32 j, const fp k)
{
	if (Check(i,j) == false) return;
  for (ui32 x = 0; x < 4; x++) z[i]->m[x] += z[j]->m[x]*k; 
};

void Matrix::Set(const ui32 i, const fp m1, const fp m2, const fp m3, const fp m4)
{
	if (i > 2 || z[i] == NULL) return;
	z[i]->Set(m1,m2,m3,m4);
};

// gauss algorithm
bool Matrix::Solve()
{  
  for (ui32 x = 0; x < 3; x++)
	{
restart:
		// check for xn being zero. if yes, search for a xn != 0 below and swap
		if (z[x]->m[x] == 0)
		{
			for (ui32 t = x+1; t < 3; t++)
			{        
				if (z[t]->m[x] != 0)
        {
          Swap(x, t);
          goto restart;
        }
			}
      // couldn't find another x!= 0. not solvable.
      return false;
		}
    
    // divide by x
    z[x]->Div(z[x]->m[x]);
    
    // add line to all lines below and above
    for (ui32 t = 0; t < 3; t++)
    {
      if (t == x) continue;
      Add(t, x, -(z[t]->m[x]));
    }
	};
  return true; // seems like it's solved
};
